% Function implementing equality constraints
function y = cloth_eqconstr2(array,Restlength,D,m,g)

% Convert array to 3d tensor
tensor = array2tensor(array);
N=length(tensor(:,1,1));
hulpmatrix = zeros(4,3);

% Every point has a penalty for the distance from the desired coordinates
% punt=squeeze(tensor(1,1,:));
% hulpmatrix(1,:)=punt'-[-N/2 -N/2 1];
% punt=squeeze(tensor(N,1,:));
% hulpmatrix(2,:)=punt'-[-N/2 N/2 1];
% punt=squeeze(tensor(1,N,:));
% hulpmatrix(3,:)=punt'-[N/2 -N/2 1];
% punt=squeeze(tensor(N,N,:));
% hulpmatrix(4,:)=punt'-[N/2 N/2 1];

Aeq=zeros(2,N^2);
Aeq(1,N^2)=1;
Aeq(2,(N^2-(N-1)))=1;
Aeq=kron(Aeq,eye(3));
beq=[0 N/2 3 0 -N/2 3];
y = Aeq*array-beq';
% Aeq=zeros(3,N^2);
% Aeq(1,1)=1; 
% Aeq(2,N)=1;
% Aeq(3,N^2)=1;
% Aeq=kron(Aeq,eye(3));
% beq=[-N/2 -N/2 1 -N/2 N/2 1 N/2 N/2 1];
% y = Aeq*array-beq';

% punt=squeeze(tensor(ceil(N/2),ceil(N/2),:));
% hulpmatrix(5,:)=punt'-[N/2 N/2 0.95];

% y = reshape(hulpmatrix,[],1);